#include <cstdio>
#include <iostream>
#include <vector>
#include <opencv/cv.h>
#include <opencv/cxcore.h>
#include <opencv/highgui.h>
#include <cmath>
#include <limits.h>
#include "color_point.h"
#include "PoseEstimation.h"
#include "MarkerTracker.h"


using namespace std;

//#define DEBUG_LINES
//#define DEBUG_CORNERS
//#define SHOW_WINDOWS

#define GET_PIXEL(IMAGE,X,Y) ( unsigned char ) (IMAGE->imageData[Y * IMAGE->widthStep  + X])

const char* CAMERA_IMAGE_WINDOW = "Camera Image";
const char* STRIPE_WINDOW = "Stripe";
const char* STRIPE_DER_WINDOW = "Stripe Der";
const char* MARKER_WINDOW = "Marker";

const int MAX_THRESHOLD_TRACKBAR_VALUE = 255;
const int POLY_CORNERS_COUNT = 4;
const int MIN_BOUNDING_RECT_SIZE = 30;
const int LINE_SEGMENTS = 7;

//const double MARKER_SIZE = 0.031;
const double MARKER_SIZE = 0.026;
//const double MARKER_SIZE = 0.023;

int marker_treshold = 100;

bool hideMarker = true;
int calibratingAdditionalMarkers = 0;

std::vector<Point> blueMarkers;

std::map<short, float*> markers_map;

// Some global variables
int current_threshold = MAX_THRESHOLD_TRACKBAR_VALUE / 2;

CvCapture* capture;

IplImage *frame_image;
IplImage *grayscale_image;
IplImage *threshold_image;
IplImage *marker_image;

IplImage *stripe_image = NULL;
IplImage *stripe_derivative_image = NULL;

CvPoint2D32f marker_points[4];

bool planeCalibEnabled = false;

CvScalar averageColor(IplImage* image, CvRect area)
{
	int R=0, G=0, B=0;

	int x2 = area.x + area.width;
	int y2 = area.y + area.height;

	for (int x1 = area.x; x1<x2; ++x1)
		for (int y1 = area.y; y1<y2; ++y1)
		{
			if (x1 >= 0 && x1 < image->width && y1 >= 0 && y1 < image->height)
			{
				CvScalar s = cvGet2D(image, y1, x1);
				R += s.val[2];
				G += s.val[1];
				B += s.val[0];
			}
		}

	int size = area.width * area.height;

	//cvRectangle(image, cvPoint(area.x, area.y), cvPoint(area.x+area.width, area.y+area.height), CV_RGB(255, 255, 0));

	return CV_RGB(R/size, G/size, B/size);
}

CvScalar interpolateColor(CvScalar cUL, CvScalar cBL, CvScalar cUR, CvScalar cBR, float px, float py)
{
	float result[3];
	for (int i=0; i<3; ++i)
	{
		float cL = py*cBL.val[i] + (1-py)*cUL.val[i];
		float cR = py*cBR.val[i] + (1-py)*cUR.val[i];
		result[i] = px*cR + (1-px)*cL;
	}
	return cvScalar(result[0], result[1], result[2], 0);
}

void doHideMarker(IplImage* frame_image, CvPoint2D32f* corners)
{
	CvPoint points[4];
	for (int i=0; i<4; ++i)
	{
		points[i].x = corners[i].x + 320;
		points[i].y = 240 - corners[i].y;
	}

	CvSeq seq_header, *seq = &seq_header;
	CvSeqBlock block;
	cvMakeSeqHeaderForArray( CV_SEQ_ELTYPE_POINT, sizeof(seq_header), sizeof(CvPoint), points,	4, &seq_header, &block );

	CvRect bounds = cvBoundingRect(seq, 0);

	// Correct the bounds a bit i order to compensate for the white border of the marker
	float padding_x = bounds.width * 0.3f;
	float padding_y = bounds.height * 0.3f;

	bounds.x -= padding_x/2;
	bounds.y -= padding_y/2;
	bounds.width += padding_x;
	bounds.height += padding_y;

	// Sample the colors around the corners
	CvScalar cUL, cUR, cBL, cBR;
	int sx = 5;
	int sy = 5;
	cUL = averageColor(frame_image, cvRect(bounds.x-sx, bounds.y+sx, sx, sy));
	cBL = averageColor(frame_image, cvRect(bounds.x-sx, bounds.y + bounds.height-sy-sy, sx, sy));
	cUR = averageColor(frame_image, cvRect(bounds.x+bounds.width, bounds.y+sx, sx, sy));
	cBR = averageColor(frame_image, cvRect(bounds.x+bounds.width, bounds.y + bounds.height-sy-sy, sx, sy));

	//cvRectangle(frame_image, cvPoint(bounds.x, bounds.y), cvPoint(bounds.x+bounds.width, bounds.y+bounds.height), cBR, CV_FILLED);
	int x2 = bounds.x + bounds.width;
	int y2 = bounds.y + bounds.height;

	float px = 0, py = 0;
	float dx = 1.0f / bounds.width;
	float dy = 1.0f / bounds.height;

	for (int x = bounds.x; x < x2; ++x) 
	{
		py = 0;
		for (int y = bounds.y; y < y2; ++y)
		{
			if (x >= 0 && x < frame_image->width && y >= 0 && y < frame_image->height)
			{
				CvScalar cc = interpolateColor(cUL, cBL, cUR, cBR, px, py);
				cvSet2D(frame_image, y, x, cc);
			}
			py += dy;

		}
		px += dx;
	}

	//cvDrawRect(frame_image, cvPoint(bounds.x, bounds.y), cvPoint(bounds.x+bounds.width, bounds.y+bounds.height), CV_RGB(255, 255, 0));
}

void clearMarkersMap()
{
	std::map<short, float*>::iterator iter;

	for (iter = markers_map.begin(); iter != markers_map.end(); ++iter)
		delete[] iter->second;

	markers_map.clear();
}

unsigned char get_pixel(IplImage* image, int x, int y)
{
	return ( unsigned char ) (image->imageData[y * image->widthStep  + x]);
}

int subpixSampleSafe ( const IplImage* pSrc, double px, double py )
{
	int x = int( floorf ( px ) );
	int y = int( floorf ( py ) );

	if ( x < 0 || x >= pSrc->width  - 1 ||
		 y < 0 || y >= pSrc->height - 1 )
		return 127;

	int dx = int ( 256 * ( px - floorf ( px ) ) );
	int dy = int ( 256 * ( py - floorf ( py ) ) );

	unsigned char* i = ( unsigned char* ) ( ( pSrc->imageData + y * pSrc->widthStep ) + x );
	int a = i[ 0 ] + ( ( dx * ( i[ 1 ] - i[ 0 ] ) ) >> 8 );
	i += pSrc->widthStep;
	int b = i[ 0 ] + ( ( dx * ( i[ 1 ] - i[ 0 ] ) ) >> 8 );
	return a + ( ( dy * ( b - a) ) >> 8 );
}

void applySobelVertical(const unsigned char* image, unsigned int width, unsigned int height, int* output)
{
	if (width != 3 || height < 5)
		return;

	int k=4;
	for (int i=1; i < height-1; ++i, k += 3)
		output[i-1] = image[k-4]*(-1) + image[k-3]*(-2) + image[k-2]*(-1) +
					  image[k+2]*1    + image[k+3]*2    + image[k+4]*1;
}

void showStripe(IplImage *stripe_image, IplImage *stripe_derivative_image, CvSize stripe_size, unsigned char* sub_image_data, int* sub_image_derivative)
{
	if (stripe_image != NULL)
		cvReleaseImage(&stripe_image);

	stripe_image = cvCreateImage(stripe_size, IPL_DEPTH_8U, 1);

	for (int r=0; r<stripe_size.height; ++r)
		for (int c=0; c<stripe_size.width; ++c)
			((unsigned char *) (stripe_image->imageData))[r*stripe_image->widthStep + c] = sub_image_data[r*stripe_size.width + c];

	int max_value = INT_MIN, min_value = INT_MAX;

	for (int k=0; k < stripe_size.height-2; ++k)
	{
		if (max_value <= sub_image_derivative[k])
			max_value = sub_image_derivative[k];

		if (min_value >= sub_image_derivative[k])
			min_value = sub_image_derivative[k];
	}

	stripe_derivative_image = cvCreateImage(cvSize(stripe_size.width-2, stripe_size.height-2), IPL_DEPTH_8U, 1);
	double norm_factor = 255.0/(max_value-min_value);

	for (int r=0; r<stripe_size.height-2; ++r)
		((unsigned char *) (stripe_derivative_image->imageData))[r*stripe_derivative_image->widthStep] = (unsigned char) ((sub_image_derivative[r] - min_value)*norm_factor);

#ifdef SHOW_WINDOWS
	cvShowImage(STRIPE_WINDOW, stripe_image);
	cvShowImage(STRIPE_DER_WINDOW, stripe_derivative_image);
#endif
}

void processPolygonSide(IplImage *frame_image, IplImage *grayscale_image, int j, CvPoint* p1, CvPoint* p2, double dx, double dy, CvSize stripe_size, CvPoint2D32f stripe_x, CvPoint2D32f stripe_y, CvPoint2D32f *precise_points, IplImage *stripe_image, IplImage *stripe_derivative_image, bool *stripe_filled)
{
	// draw the point with pixel accuracy
	CvPoint center = cvPoint(p1->x + j*dx, p1->y + j*dy);
	#ifdef DEBUG_LINES
	cvCircle(frame_image, center, 2, cvScalar(255, 0, 0), 2);
	#endif

	// extract sub image stripe
	unsigned char* sub_image_data = new unsigned char[stripe_size.width * stripe_size.height];
	unsigned int sub_image_pos = 0;

	CvPoint2D32f centerf = cvPoint2D32f(p1->x+j*dx-0.5, p1->y+j*dy-0.5);

	int row_from = -stripe_size.height/2;
	int row_to = row_from + stripe_size.height;

	int col_from = -stripe_size.width/2;
	int col_to = col_from + stripe_size.width;

	for (int row = row_from; row < row_to; ++row)
		for (int col = col_from; col < col_to; ++col)
		{
			double sx = (double)center.x + ((double)col*stripe_x.x) + ((double)row*stripe_y.x);
			double sy = (double)center.y + ((double)col*stripe_x.y) + ((double)row*stripe_y.y);
			int pixel = subpixSampleSafe(grayscale_image, sx, sy);
			sub_image_data[sub_image_pos++] = pixel;
			#ifdef DEBUG_LINES
			cvCircle(frame_image, cvPoint(sx,sy), 1, (*stripe_filled ? CV_RGB(0, 255, 255) : CV_RGB(255, 0, 255)));
			#endif
		}

	// calculate point with sub pixel accuracy
	int* sub_image_derivative = new int[(stripe_size.width-2) * (stripe_size.height-2)];

	applySobelVertical(sub_image_data, stripe_size.width, stripe_size.height, sub_image_derivative);

	int dh = stripe_size.height-2;
	int max_value = INT_MIN, max_id = -1;

	for (int k=0; k < dh; ++k)
	{
		if (max_value <= sub_image_derivative[k])
		{
			max_value = sub_image_derivative[k];
			max_id = k;
		}
	}

	double optimum_position = 0;

	//if (max_id == 0 || max_id == dh-1)
	//	optimum_position = max_id;
	//else // x = (A-C) / 2*(A+C-2*B)
	//{
	//	double devisor = (2 * (sub_image_derivative[max_id-1] + sub_image_derivative[max_id+1] - 2*sub_image_derivative[max_id]));

	//	if (devisor == 0)
	//		optimum_position = max_id;
	//	else
	//		optimum_position = (sub_image_derivative[max_id-1] - sub_image_derivative[max_id+1]) / devisor;

	//	printf("%f\n", optimum_position);
	//}

	int A = max_id <= 0 ? 0 : sub_image_derivative[max_id-1];
	int B = sub_image_derivative[max_id];
	int C = max_id >= dh-1 ? 0 : sub_image_derivative[max_id+1];

	double devisor = (2 * (A + C - 2*B));

	if (devisor == 0)
		optimum_position = max_id;
	else
		optimum_position = (A - C) / devisor;



	int offset = (int)(max_id - (stripe_size.height>>1));

	precise_points[j-1].x = center.x + ((double)offset+optimum_position)*stripe_y.x;
	precise_points[j-1].y = center.y + ((double)offset+optimum_position)*stripe_y.y;

	//printf("Old Point (%f,%f)\tNew Point (%f,%f)\n", centerf.x, centerf.y, precise_points[j-1].x, precise_points[j-1].y);
	//printf("Diff\t%f\t%f\n", centerf.x-precise_points[j-1].x, centerf.y-precise_points[j-1].y);

	// visualize
	if (!(*stripe_filled))
	{
		*stripe_filled = true;
		showStripe(stripe_image, stripe_derivative_image, stripe_size, sub_image_data, sub_image_derivative);
	}

	delete[] sub_image_data;
	delete[] sub_image_derivative;
}

void findEdge(IplImage *frame_image, IplImage *grayscale_image, CvPoint* p1, CvPoint* p2, IplImage *stripe_image, IplImage *stripe_derivative_image, bool *stripe_filled, float* edge)
{
	double dx = ((double) (p2->x - p1->x))/LINE_SEGMENTS;
	double dy = ((double) (p2->y - p1->y))/LINE_SEGMENTS;
	double length = sqrt(dx*dx + dy*dy);

	CvPoint2D32f *precise_points = new CvPoint2D32f[LINE_SEGMENTS-1];

	// init stripe size and vectors
	CvSize stripe_size = cvSize(3, length < 5 ? 5 : length);
	CvPoint2D32f stripe_x = cvPoint2D32f(dx / length, dy / length);
	CvPoint2D32f stripe_y = cvPoint2D32f(stripe_x.y, -stripe_x.x);

	for (int j=1; j<LINE_SEGMENTS; ++j)
		processPolygonSide(frame_image, grayscale_image, j, p1, p2, dx, dy, stripe_size, stripe_x, stripe_y, precise_points, stripe_image, stripe_derivative_image, stripe_filled);

	cvFitLine2D(precise_points, LINE_SEGMENTS-1, CV_DIST_L2, 0, 0.01, 0.01, edge);

	delete[] precise_points;
}

void drawLine(IplImage* frame_image, float* l)
{
	CvPoint lp1 = cvPoint(0, (-l[1]*l[2]+l[0]*l[3])/l[0]);
	CvPoint lp2 = cvPoint(1000, (l[1]*1000-l[1]*l[2]+l[0]*l[3])/l[0]);

	cvDrawLine(frame_image, lp1, lp2, CV_RGB(0,255,255));
}

void intersectLines(float* l1, float* l2, CvPoint2D32f* point)
{
	float k = (l2[1]*(l2[2]-l1[2]) - l2[0]*(l2[3]-l1[3])) / (l2[1]*l1[0] - l2[0]*l1[1]);
	point->x = l1[2] + k*l1[0];
	point->y = l1[3] + k*l1[1];
}

int identifyMarker(IplImage *image, int *rotation = NULL)
{
	// Check if the border is black. If it is not - return -1
	for (int i=0; i<6; ++i)
		if (GET_PIXEL(image, i, 0) > 0 || GET_PIXEL(image, 5, i) > 0 ||	GET_PIXEL(image, i, 5) > 0 || GET_PIXEL(image, 0, i) > 0)
			return -1;

	unsigned short id_bytes[16] = {0};

	// Determine the marker ID
	for (int i=1; i<5; ++i)
	{
		char t = 0;
		for (int j=1; j<5; ++j)
		{
			id_bytes[i-1 +  0] <<= 1;
			id_bytes[i-1 +  0] |= (get_pixel(image, 5-j, i) ? 0 : 1);

			id_bytes[i-1 +  4] <<= 1;
			id_bytes[i-1 +  4] |= (get_pixel(image, 5-i, 5-j) ? 0 : 1);

			id_bytes[i-1 +  8] <<= 1;
			id_bytes[i-1 +  8] |= (get_pixel(image, j, 5-i) ? 0 : 1);

			id_bytes[i-1 + 12] <<= 1;
			id_bytes[i-1 + 12] |= (get_pixel(image, i, j) ? 0 : 1);
		}
	}

	int min_id = INT_MAX, min_id_index = -1;
	for (int i=0; i<4; ++i)
	{
		int tmp_id = id_bytes[i*4 + 0] << 12 | id_bytes[i*4 + 1] << 8 | id_bytes[i*4 + 2] << 4 | id_bytes[i*4 + 3];
		if (tmp_id < min_id)
		{
			min_id = tmp_id;
			min_id_index = i;
		}
	}

	if (rotation != NULL)
		*rotation = min_id_index;

	return min_id;
}

void processFrame(IplImage *frame_image, IplImage *grayscale_image, IplImage *threshold_image, IplImage **marker_image, IplImage *stripe_image, IplImage *stripe_derivative_image, CvPoint2D32f* marker_points)
{
	// Clear the marker ids from previouse step
	clearMarkersMap();

	bool stripe_filled = false;

	// Find the conour
	CvMemStorage* contour_storage = cvCreateMemStorage();
	CvSeq* contour;
	cvFindContours(threshold_image, contour_storage, &contour, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

	for (; contour != NULL; contour = contour->h_next)
	{
		CvMemStorage* poly_storage = cvCreateMemStorage();
		CvSeq* polygons = cvApproxPoly(contour, contour->header_size, poly_storage, CV_POLY_APPROX_DP, cvContourPerimeter(contour) * 0.02, 0);

		CvPoint2D32f corners[POLY_CORNERS_COUNT];

		if (polygons->total == POLY_CORNERS_COUNT )
		{
			CvRect rect = cvBoundingRect(polygons);
			// Filter too small or non-convex polygons
			if (rect.height > MIN_BOUNDING_RECT_SIZE && rect.width > MIN_BOUNDING_RECT_SIZE && cvCheckContourConvexity(polygons))
			{
				CvPoint points[POLY_CORNERS_COUNT];
				cvCvtSeqToArray(polygons, points);

				float edges[POLY_CORNERS_COUNT][4];

				for (int i=0; i<POLY_CORNERS_COUNT; ++i)
				{
					CvPoint* p1 = &points[i];
					CvPoint* p2 = &points[(i+1)%POLY_CORNERS_COUNT];

					#ifdef DEBUG_CORNERS
					cvCircle(frame_image, *p1, 1, CV_RGB(255, 0, 0), 2);
					#endif

					#ifdef DEBUG_LINES
					cvLine(frame_image, *p1, *p2, CV_RGB(255, 0, 0), 2);
					#endif

					findEdge(frame_image, grayscale_image, p1, p2, stripe_image, stripe_derivative_image, &stripe_filled, edges[i]);

					#ifdef DEBUG_LINES
					drawLine(frame_image, edges[i]);
					#endif
				}

				// Intersect the lines of each two consecutive edges in order to get the exact corner point
				for (int i=0; i<POLY_CORNERS_COUNT; ++i)
				{
					intersectLines(edges[i], edges[(i+1) % POLY_CORNERS_COUNT], &corners[i]);

					#ifdef DEBUG_CORNERS
					cvCircle(frame_image, cvPointFrom32f(corners[i]), 1, CV_RGB(255, 255, 0), 2);
					#endif
				}

				// Calculate the perspective transform and warp the image
				CvMat* perspective_matrix = cvCreateMat(3, 3, CV_32F);
				cvGetPerspectiveTransform(marker_points, corners, perspective_matrix);

				IplImage* tmp_marker = cvCreateImage(cvSize(6, 6), IPL_DEPTH_8U, 1);
				IplImage* tmp_marker_bin = cvCreateImage(cvSize(6, 6), IPL_DEPTH_8U, 1);
				cvWarpPerspective(grayscale_image, tmp_marker, perspective_matrix, CV_WARP_INVERSE_MAP);

				// Make the image binary
				cvThreshold(tmp_marker, tmp_marker_bin, marker_treshold, 255, CV_THRESH_BINARY);
				cvReleaseImage(&tmp_marker);

				// Identify the marker
				int rotation;
				short marker_id = identifyMarker(tmp_marker_bin, &rotation);

				if (marker_id > 0 && marker_id < 0xffff)
				{
					// printf("Marker found! ID = %x (%d), rotation = %d\n", (unsigned short) marker_id, marker_id, rotation*90);

					cvReleaseImage(marker_image);
					marker_image = &tmp_marker_bin;

					#ifdef SHOW_WINDOWS
					cvShowImage(MARKER_WINDOW, *marker_image);
					#endif

					CvPoint2D32f corrected_corners[POLY_CORNERS_COUNT];

					for (int i=0; i<POLY_CORNERS_COUNT; ++i)
						corrected_corners[(i+rotation)%POLY_CORNERS_COUNT] = cvPoint2D32f(corners[i].x - 320, 240 - corners[i].y);

					float* pose_matrix = new float[16];

					// Estimate pose!
					estimateSquarePose(pose_matrix, corrected_corners, MARKER_SIZE);
				

				//------------------------------
				//***Georg**********************

					if (calibratingAdditionalMarkers > 0)
					{
						--calibratingAdditionalMarkers;

						IplImage* blue_img = cvCreateImage(cvSize(frame_image->width, frame_image->height), IPL_DEPTH_8U, 1);	

						color_point(frame_image, blue_img, blueMarkers);

						cvNamedWindow("BLUE_IMG",1);
						cvShowImage("BLUE_IMG", blue_img);
						cvReleaseImage(&blue_img);

						int size = blueMarkers.size();
						cout << "size of array: " << size << endl;
					}
					else
						cvDestroyWindow("BLUE_IMG");

					/*for (int i=0;i<size;i++)
						cout << "point x/y " << blueMarkers[i].x << ", " << blueMarkers[i].y << endl;*/

				//***end Georg******************
				//------------------------------

					//for (int i=0; i<16; ++i)
					//{
					//	printf("%f\t", pose_matrix[i]);
					//	if (i % 4 == 3)
					//		printf("\n");
					//}

					// Estimate the rotation with Rodrigez formula
					float alpha = acos(0.5*(pose_matrix[10] - 1 + sqrt((pose_matrix[10]+1)*(pose_matrix[10]+1) - (pose_matrix[4]-pose_matrix[1])*(pose_matrix[4]-pose_matrix[1]))));
					float t = 2*sin(alpha);

					float r_x = (pose_matrix[9]-pose_matrix[6])/t;
					float r_y = (pose_matrix[2]-pose_matrix[8])/t;
					float r_z = (pose_matrix[4]-pose_matrix[1])/t;

					 //printf("Rodrigez: x = %f\ty = %f\tz = %f\ta = %f\n\n", r_x, r_y, r_z, alpha*180/3.14f);

					markers_map[marker_id] = pose_matrix;

					if (hideMarker)
						doHideMarker(frame_image, corrected_corners);
				}
				else
					cvReleaseImage(&tmp_marker_bin);
			}
		}
		cvReleaseMemStorage(&poly_storage);
	}

	cvReleaseMemStorage(&contour_storage);
#ifdef SHOW_WINDOWS
	cvShowImage(CAMERA_IMAGE_WINDOW, frame_image);
#endif
}


void init()
{
	capture = cvCaptureFromCAM(-1);

	if (NULL == capture)
	{
		printf("Unable to capture camera image. Tring to open MarkerMovie.mpg...\n");
		capture = cvCaptureFromAVI("MarkerMovie.mpg");
	}

	if (NULL == capture)
	{
		printf("Unable to open video file! Exiting...\n");
		return;
	}

#ifdef SHOW_WINDOWS
	cvNamedWindow(CAMERA_IMAGE_WINDOW, CV_WND_PROP_AUTOSIZE);
	cvNamedWindow(STRIPE_WINDOW, CV_WINDOW_NORMAL);
	cvNamedWindow(STRIPE_DER_WINDOW, CV_WINDOW_NORMAL);
	cvNamedWindow(MARKER_WINDOW, CV_WINDOW_NORMAL);
#endif

	CvSize image_size = cvSize(cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH), cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT));

	frame_image = NULL;
	grayscale_image = cvCreateImage(image_size, IPL_DEPTH_8U, 1);
	threshold_image = cvCreateImage(image_size, IPL_DEPTH_8U, 1);
	marker_image = cvCreateImage(cvSize(6, 6), IPL_DEPTH_8U, 1);

	cvCreateTrackbar("Marker Threshold", MARKER_WINDOW, &marker_treshold, 255, NULL);

	// Init the marker corners
	marker_points[0] = cvPoint2D32f(-0.5, -0.5);
	marker_points[1] = cvPoint2D32f(-0.5,  5.5);
	marker_points[2] = cvPoint2D32f( 5.5,  5.5);
	marker_points[3] = cvPoint2D32f( 5.5, -0.5);
}

void finish()
{
	if (stripe_image != NULL)
		cvReleaseImage(&stripe_image);

	cvReleaseImage(&grayscale_image);
	cvReleaseImage(&threshold_image);
	cvReleaseImage(&stripe_image);
	cvReleaseImage(&stripe_derivative_image);
	cvReleaseImage(&marker_image);

	cvDestroyAllWindows();

	cvReleaseCapture(&capture);
}

void mainLoop()
{
	blueMarkers.clear();

	frame_image = cvQueryFrame(capture);

	if (frame_image == NULL)
		return;

	cvConvertImage(frame_image, grayscale_image);

	// Adaptive Threshold
	cvAdaptiveThreshold(grayscale_image, threshold_image, 255, 0, 0, 33);

	// Process the current frame
	processFrame(frame_image, grayscale_image, threshold_image, &marker_image, stripe_image, stripe_derivative_image, marker_points);
}
